/*
 * Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
 *
 * This file is part of SeMoLearning.
 *
 * SeMoLearning is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * SeMoLearning is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with SeMoLearning.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "semol/core/DBDriver.h"

#include "semol/core/Node.h"

#include "DatabaseSchema_sql.h"

#include <set>

#include <opencv2/highgui/highgui.hpp>

#include <utilite/ULogger.h>
#include <utilite/UTimer.h>
#include <utilite/UConversion.h>
#include <utilite/UFile.h>
#include <utilite/UMutex.h>

namespace semol {

DBDriver::DBDriver(const ParametersMap & parameters) :
	_ppDb(0),
	_dbInMemory(Parameters::defaultDbSqlite3InMemory()),
	_cacheSize(Parameters::defaultDbSqlite3CacheSize()),
	_journalMode(Parameters::defaultDbSqlite3JournalMode()),
	_synchronous(Parameters::defaultDbSqlite3Synchronous()),
	_tempStore(Parameters::defaultDbSqlite3TempStore()),
	_imagesCompressed(Parameters::defaultDbImagesCompressed()),
	_dbMutex(new UMutex())
{
	ULOGGER_DEBUG("treadSafe=%d", sqlite3_threadsafe());
	this->parseParameters(parameters);
}

DBDriver::~DBDriver()
{
	this->closeConnection();
	delete _dbMutex;
}

void DBDriver::parseParameters(const ParametersMap & parameters)
{
	ParametersMap::const_iterator iter;
	if((iter=parameters.find(Parameters::kDbSqlite3CacheSize())) != parameters.end())
	{
		this->setCacheSize(std::atoi((*iter).second.c_str()));
	}
	if((iter=parameters.find(Parameters::kDbSqlite3JournalMode())) != parameters.end())
	{
		this->setJournalMode(std::atoi((*iter).second.c_str()));
	}
	if((iter=parameters.find(Parameters::kDbSqlite3Synchronous())) != parameters.end())
	{
		this->setSynchronous(std::atoi((*iter).second.c_str()));
	}
	if((iter=parameters.find(Parameters::kDbSqlite3TempStore())) != parameters.end())
	{
		this->setTempStore(std::atoi((*iter).second.c_str()));
	}
	if((iter=parameters.find(Parameters::kDbSqlite3InMemory())) != parameters.end())
	{
		this->setDbInMemory(uStr2Bool((*iter).second.c_str()));
	}
	if((iter=parameters.find(Parameters::kDbImagesCompressed())) != parameters.end())
	{
		this->setImagesCompressed(uStr2Bool((*iter).second.c_str()));
	}
}

void DBDriver::setCacheSize(unsigned int cacheSize)
{
	if(this->isConnected())
	{
		_cacheSize = cacheSize;
		std::string query = "PRAGMA cache_size = ";
		query += uNumber2Str(_cacheSize) + ";";
		this->executeNoResult(query.c_str());
	}
}

void DBDriver::setJournalMode(int journalMode)
{
	if(journalMode >= 0 && journalMode < 5)
	{
		_journalMode = journalMode;
		if(this->isConnected())
		{
			switch(_journalMode)
			{
			case 4:
				this->executeNoResult("PRAGMA journal_mode = OFF;");
				break;
			case 3:
				this->executeNoResult("PRAGMA journal_mode = MEMORY;");
				break;
			case 2:
				this->executeNoResult("PRAGMA journal_mode = PERSIST;");
				break;
			case 1:
				this->executeNoResult("PRAGMA journal_mode = TRUNCATE;");
				break;
			case 0:
			default:
				this->executeNoResult("PRAGMA journal_mode = DELETE;");
				break;
			}
		}
	}
	else
	{
		ULOGGER_ERROR("Wrong journal mode (%d)", journalMode);
	}
}

void DBDriver::setSynchronous(int synchronous)
{
	if(synchronous >= 0 && synchronous < 3)
	{
		_synchronous = synchronous;
		if(this->isConnected())
		{
			switch(_synchronous)
			{
			case 0:
				this->executeNoResult("PRAGMA synchronous = OFF;");
				break;
			case 1:
				this->executeNoResult("PRAGMA synchronous = NORMAL;");
				break;
			case 2:
			default:
				this->executeNoResult("PRAGMA synchronous = FULL;");
				break;
			}
		}
	}
	else
	{
		ULOGGER_ERROR("Wrong synchronous value (%d)", synchronous);
	}
}

void DBDriver::setTempStore(int tempStore)
{
	if(tempStore >= 0 && tempStore < 3)
	{
		_tempStore = tempStore;
		if(this->isConnected())
		{
			switch(_tempStore)
			{
			case 2:
				this->executeNoResult("PRAGMA temp_store = MEMORY;");
				break;
			case 1:
				this->executeNoResult("PRAGMA temp_store = FILE;");
				break;
			case 0:
			default:
				this->executeNoResult("PRAGMA temp_store = DEFAULT;");
				break;
			}
		}
	}
	else
	{
		ULOGGER_ERROR("Wrong tempStore value (%d)", tempStore);
	}
}

void DBDriver::setImagesCompressed(bool imagesCompressed)
{
	_imagesCompressed = imagesCompressed;
}

void DBDriver::lock() const
{
	_dbMutex->lock();
}

void DBDriver::unlock() const
{
	_dbMutex->unlock();
}

void DBDriver::setDbInMemory(bool dbInMemory)
{
	if(dbInMemory != _dbInMemory)
	{
		if(this->isConnected())
		{
			// Hard reset...
			this->closeConnection();
			_dbInMemory = dbInMemory;
			this->openConnection(this->getPath());
		}
		else
		{
			_dbInMemory = dbInMemory;
		}
	}
}

/*
** This function is used to load the contents of a database file on disk
** into the "main" database of open database connection pInMemory, or
** to save the current contents of the database opened by pInMemory into
** a database file on disk. pInMemory is probably an in-memory database,
** but this function will also work fine if it is not.
**
** Parameter zFilename points to a nul-terminated string containing the
** name of the database file on disk to load from or save to. If parameter
** isSave is non-zero, then the contents of the file zFilename are
** overwritten with the contents of the database opened by pInMemory. If
** parameter isSave is zero, then the contents of the database opened by
** pInMemory are replaced by data loaded from the file zFilename.
**
** If the operation is successful, SQLITE_OK is returned. Otherwise, if
** an error occurs, an SQLite error code is returned.
*/
int DBDriver::loadOrSaveDb(sqlite3 *pInMemory, const std::string & fileName, int isSave) const
{
  int rc;                   /* Function return code */
  sqlite3 *pFile = 0;           /* Database connection opened on zFilename */
  sqlite3_backup *pBackup = 0;  /* Backup object used to copy data */
  sqlite3 *pTo = 0;             /* Database to copy to (pFile or pInMemory) */
  sqlite3 *pFrom = 0;           /* Database to copy from (pFile or pInMemory) */

  /* Open the database file identified by zFilename. Exit early if this fails
  ** for any reason. */
  rc = sqlite3_open(fileName.c_str(), &pFile);
  if( rc==SQLITE_OK ){

    /* If this is a 'load' operation (isSave==0), then data is copied
    ** from the database file just opened to database pInMemory.
    ** Otherwise, if this is a 'save' operation (isSave==1), then data
    ** is copied from pInMemory to pFile.  Set the variables pFrom and
    ** pTo accordingly. */
    pFrom = (isSave ? pInMemory : pFile);
    pTo   = (isSave ? pFile     : pInMemory);

    /* Set up the backup procedure to copy from the "main" database of
    ** connection pFile to the main database of connection pInMemory.
    ** If something goes wrong, pBackup will be set to NULL and an error
    ** code and  message left in connection pTo.
    **
    ** If the backup object is successfully created, call backup_step()
    ** to copy data from pFile to pInMemory. Then call backup_finish()
    ** to release resources associated with the pBackup object.  If an
    ** error occurred, then  an error code and message will be left in
    ** connection pTo. If no error occurred, then the error code belonging
    ** to pTo is set to SQLITE_OK.
    */
    pBackup = sqlite3_backup_init(pTo, "main", pFrom, "main");
    if( pBackup ){
      (void)sqlite3_backup_step(pBackup, -1);
      (void)sqlite3_backup_finish(pBackup);
    }
    rc = sqlite3_errcode(pTo);
  }

  /* Close the database connection opened on database file zFilename
  ** and return the result of this function. */
  (void)sqlite3_close(pFile);
  return rc;
}

void DBDriver::closeConnection()
{
	UScopeMutex scopeMutex(_dbMutex);
	if(_ppDb)
	{
		int rc = SQLITE_OK;
		// make sure that all statements are finalized
		sqlite3_stmt * pStmt;
		while( (pStmt = sqlite3_next_stmt(_ppDb, 0))!=0 )
		{
			rc = sqlite3_finalize(pStmt);
			if(rc != SQLITE_OK)
			{
				UERROR("");
			}
		}

		if(_dbInMemory)
		{
			UTimer timer;
			timer.start();
			UDEBUG("Saving DB ...");
			rc = loadOrSaveDb(_ppDb, this->getPath(), 1); // Save memory to file
			if(rc != SQLITE_OK)
			{
				UERROR("");
			}
			UDEBUG("Saving DB time = %fs", timer.ticks());
		}

		// Then close (delete) the database connection
		sqlite3_close(_ppDb);
		_ppDb = 0;
	}
}

bool DBDriver::openConnection(const std::string & url, bool overwritten)
{
	UScopeMutex scopeMutex(_dbMutex);

	this->closeConnection();

	_path = url;

	// Open a database connection
	_ppDb = 0;

	if(url.empty())
	{
		UERROR("url is empty...");
		return false;
	}

	int rc = SQLITE_OK;
	bool dbFileExist = UFile::exists(url.c_str());
	if(dbFileExist && overwritten)
	{
		UFile::erase(url.c_str());
		dbFileExist = false;
	}

	if(_dbInMemory)
	{
		UINFO("Using database \"%s\" in the memory.", url.c_str());
		rc = sqlite3_open_v2(":memory:", &_ppDb, SQLITE_OPEN_READWRITE | SQLITE_OPEN_CREATE, 0);
	}
	else
	{
		UINFO("Using database \"%s\" from the hard drive.", url.c_str());
		rc = sqlite3_open_v2(url.c_str(), &_ppDb, SQLITE_OPEN_READWRITE | SQLITE_OPEN_CREATE, 0);
	}
	if(rc != SQLITE_OK)
	{
		UERROR("DB error : %s", sqlite3_errmsg(_ppDb));
		_ppDb = 0;
		return false;
	}

	if(_dbInMemory && dbFileExist)
	{
		UTimer timer;
		timer.start();
		ULOGGER_DEBUG("Loading DB ...");
		rc = loadOrSaveDb(_ppDb, url, 0); // Load memory from file
		ULOGGER_INFO("Loading DB time = %fs, (%s)", timer.ticks(), url.c_str());
		if(rc != SQLITE_OK)
		{
			UERROR("DB error 2 : %s", sqlite3_errmsg(_ppDb));
			sqlite3_close(_ppDb);
			_ppDb = 0;
			return false;
		}
	}

	if(!dbFileExist)
	{
		ULOGGER_INFO("Database \"%s\" doesn't exist, creating a new one...", url.c_str());
		// Create the database
		std::string schema = DATABASESCHEMA_SQL;
		schema = uHex2Str(schema);
		this->executeNoResult(schema.c_str());
		UDEBUG("Database schema created.");
	}

	//Set database optimizations
	this->setCacheSize(_cacheSize); // this will call the SQL
	this->setJournalMode(_journalMode); // this will call the SQL
	this->setSynchronous(_synchronous); // this will call the SQL
	this->setTempStore(_tempStore); // this will call the SQL

	return true;
}

void DBDriver::beginTransaction() const
{
	_dbMutex->lock();
	ULOGGER_DEBUG("");
	this->executeNoResult("BEGIN TRANSACTION;");
}

void DBDriver::commit() const
{
	ULOGGER_DEBUG("");
	this->executeNoResult("COMMIT;");
	_dbMutex->unlock();
}

void DBDriver::saveOrUpdate(const std::vector<Node *> & signatures) const
{
	ULOGGER_DEBUG("");
	std::list<Node *> toSave;
	std::list<Node *> toUpdate;
	if(this->isConnected() && signatures.size())
	{
		for(std::vector<Node *>::const_iterator i=signatures.begin(); i!=signatures.end();++i)
		{
			if((*i)->isSaved())
			{
				toUpdate.push_back(*i);
			}
			else
			{
				toSave.push_back(*i);
			}
		}

		if(toUpdate.size())
		{
			this->update(toUpdate);
		}
		if(toSave.size())
		{
			this->save(toSave);
		}
	}
}

void DBDriver::saveOrUpdate(const std::vector<Sensor *> & sensors) const
{
	ULOGGER_DEBUG("");
	std::list<Sensor *> toSave;
	std::list<Sensor *> toUpdate;
	if(this->isConnected() && sensors.size())
	{
		for(std::vector<Sensor *>::const_iterator i=sensors.begin(); i!=sensors.end();++i)
		{
			if((*i)->isSaved())
			{
				toUpdate.push_back(*i);
			}
			else
			{
				toSave.push_back(*i);
			}
		}

		if(toUpdate.size())
		{
			this->update(toUpdate);
		}
		if(toSave.size())
		{
			this->save(toSave);
		}
	}
}

void DBDriver::addStatistics(int stMemSize, int lastSignAdded, int processMemUsed, int databaseMemUsed) const
{
	ULOGGER_DEBUG("");
	if(this->isConnected())
	{
		std::stringstream query;
		query << "INSERT INTO Statistics(STM_size,last_sign_added,process_mem_used,database_mem_used) values("
			  << stMemSize << ","
		      << lastSignAdded << ","
		      << processMemUsed << ","
			  << databaseMemUsed << ");";

		this->executeNoResult(query.str());
	}
}

//////////////////////////////////////////////
// SQLITE3 QUERIES
bool DBDriver::isConnected() const
{
	return _ppDb;
}

// In bytes
void DBDriver::executeNoResult(const std::string & sql) const
{
	UScopeMutex scopeMutex(_dbMutex);
	if(_ppDb)
	{
		UTimer timer;
		timer.start();
		int rc;
		rc = sqlite3_exec(_ppDb, sql.c_str(), 0, 0, 0);
		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

		UDEBUG("Time=%fs", timer.ticks());
	}
}

long DBDriver::getMemoryUsed() const
{
	UScopeMutex scopeMutex(_dbMutex);
	if(_dbInMemory)
	{
		return sqlite3_memory_used();
	}
	else
	{
		return UFile::length(this->getPath());
	}
}

std::list<SensorRaw> DBDriver::getRawData(int nodeId) const
{
	UScopeMutex scopeMutex(_dbMutex);
	std::list<SensorRaw> rawData;
	if(_ppDb)
	{
		UTimer timer;
		timer.start();
		int rc = SQLITE_OK;
		sqlite3_stmt * ppStmt = 0;
		std::stringstream query;

		query << "SELECT type, raw_width, raw_height, raw_data_type, raw_compressed, raw_data "
			  << "FROM Sensor "
			  << "INNER JOIN Node_To_Sensor "
			  << "ON Sensor.id = sensor_id "
			  << "WHERE node_id = " << nodeId
			  << " ORDER BY num"
			  <<";";

		rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

		int type;
		int width;
		int height;
		int dataType;
		int compressed;
		const void * data = 0;
		int dataSize = 0;
		int index = 0;

		// Process the result if one
		rc = sqlite3_step(ppStmt);
		while(rc == SQLITE_ROW)
		{
			index = 0;
			type = sqlite3_column_int(ppStmt, index++);
			width = sqlite3_column_int(ppStmt, index++);
			height = sqlite3_column_int(ppStmt, index++);
			dataType = sqlite3_column_int(ppStmt, index++);
			compressed = sqlite3_column_int(ppStmt, index++);

			data = sqlite3_column_blob(ppStmt, index);
			dataSize = sqlite3_column_bytes(ppStmt, index++);

			//Create the image
			if(dataSize>4 && data)
			{
				if(compressed)
				{
					std::vector<unsigned char> compressed(dataSize);
					memcpy(compressed.data(), data, dataSize);
#if CV_MAJOR_VERSION >=2 and CV_MINOR_VERSION >=4
					cv::Mat dataMat = cv::imdecode(compressed, cv::IMREAD_UNCHANGED);
#else
					cv::Mat dataMat = cv::imdecode(compressed, -1);
#endif
					if(dataType != dataMat.type() || width != dataMat.cols || height != dataMat.rows)
					{
						UFATAL("dataType != dataMat.type() || width != dataMat.cols || height != dataMat.rows");
					}
					rawData.push_back(SensorRaw((Sensor::Type)type, dataMat));
				}
				else
				{
					cv::Mat dataMat(height, width, dataType);
					memcpy(dataMat.data, data, dataSize);
					rawData.push_back(SensorRaw((Sensor::Type)type, dataMat));
				}
			}
			rc = sqlite3_step(ppStmt); // next result...
		}

		UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

		// Finalize (delete) the statement
		rc = sqlite3_finalize(ppStmt);
		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

		UDEBUG("Time=%fs", timer.ticks());
	}
	return rawData;
}

SensorRaw DBDriver::getSensorRawData(int sensorId) const
{
	UScopeMutex scopeMutex(_dbMutex);
	SensorRaw rawData(Sensor::kTypeEnd, cv::Mat());
	if(_ppDb)
	{
		UTimer timer;
		timer.start();
		int rc = SQLITE_OK;
		sqlite3_stmt * ppStmt = 0;
		std::stringstream query;

		query << "SELECT type, raw_width, raw_height, raw_data_type, raw_compressed, raw_data "
			  << "FROM Sensor "
			  << "WHERE id = " << sensorId
			  <<";";

		rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

		// Process the result if one
		rc = sqlite3_step(ppStmt);
		if(rc == SQLITE_ROW)
		{
			int index = 0;
			int type = sqlite3_column_int(ppStmt, index++);
			int width = sqlite3_column_int(ppStmt, index++);
			int height = sqlite3_column_int(ppStmt, index++);
			int dataType = sqlite3_column_int(ppStmt, index++);
			int compressed = sqlite3_column_int(ppStmt, index++);

			const void * data = sqlite3_column_blob(ppStmt, index);
			int dataSize = sqlite3_column_bytes(ppStmt, index++);

			//Create the image
			if(dataSize>4 && data)
			{
				if(compressed)
				{
					std::vector<unsigned char> compressed(dataSize);
					memcpy(compressed.data(), data, dataSize);
#if CV_MAJOR_VERSION >=2 and CV_MINOR_VERSION >=4
					cv::Mat dataMat = cv::imdecode(compressed, cv::IMREAD_UNCHANGED);
#else
					cv::Mat dataMat = cv::imdecode(compressed, -1);
#endif
					if(dataType != dataMat.type() || width != dataMat.cols || height != dataMat.rows)
					{
						UFATAL("dataType != dataMat.type() || width != dataMat.cols || height != dataMat.rows");
					}
					rawData = SensorRaw((Sensor::Type)type, dataMat);
				}
				else
				{
					cv::Mat dataMat(height, width, dataType);
					memcpy(dataMat.data, data, dataSize);
					rawData = SensorRaw((Sensor::Type)type, dataMat);
				}
			}
			rc = sqlite3_step(ppStmt); // next result...
		}

		UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

		// Finalize (delete) the statement
		rc = sqlite3_finalize(ppStmt);
		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

		UDEBUG("Time=%fs", timer.ticks());
	}
	return rawData;
}

std::list<Actuator> DBDriver::getActuatorData(int nodeId) const
{
	UScopeMutex scopeMutex(_dbMutex);
	std::list<Actuator> actuators;
	if(_ppDb)
	{
		UTimer timer;
		timer.start();
		int rc = SQLITE_OK;
		sqlite3_stmt * ppStmt = 0;
		std::stringstream query;

		query << "SELECT num, type, width, height, data_type, data "
			  << "FROM Actuator "
			  << "WHERE id = " << nodeId
			  << " ORDER BY num"
			  <<";";

		rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

		int num;
		int type;
		int width;
		int height;
		int dataType;
		const void * data = 0;
		int dataSize = 0;
		int index = 0;

		// Process the result if one
		rc = sqlite3_step(ppStmt);
		while(rc == SQLITE_ROW)
		{
			index = 0;
			num = sqlite3_column_int(ppStmt, index++);
			type = sqlite3_column_int(ppStmt, index++);
			width = sqlite3_column_int(ppStmt, index++);
			height = sqlite3_column_int(ppStmt, index++);
			dataType = sqlite3_column_int(ppStmt, index++);

			data = sqlite3_column_blob(ppStmt, index);
			dataSize = sqlite3_column_bytes(ppStmt, index++);

			if(dataSize>4 && data)
			{
				cv::Mat dataMat(height, width, dataType);
				memcpy(dataMat.data, data, dataSize);
				actuators.push_back(Actuator(dataMat, (Actuator::Type)type, num));
			}
			rc = sqlite3_step(ppStmt); // next result...
		}

		UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

		// Finalize (delete) the statement
		rc = sqlite3_finalize(ppStmt);
		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

		UDEBUG("Time=%fs", timer.ticks());
	}
	return actuators;
}

std::set<int> DBDriver::getAllNodeIds() const
{
	UScopeMutex scopeMutex(_dbMutex);
	std::set<int> ids;
	if(_ppDb)
	{
		UTimer timer;
		timer.start();
		int rc = SQLITE_OK;
		sqlite3_stmt * ppStmt = 0;
		std::stringstream query;

		query << "SELECT id "
			  << "FROM Node "
			  << "ORDER BY id";

		rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

		// Process the result if one
		rc = sqlite3_step(ppStmt);
		while(rc == SQLITE_ROW)
		{
			ids.insert(ids.end(), sqlite3_column_int(ppStmt, 0)); // Signature Id
			rc = sqlite3_step(ppStmt);
		}
		UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

		// Finalize (delete) the statement
		rc = sqlite3_finalize(ppStmt);
		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

		UDEBUG("Time=%f", timer.ticks());
	}
	return ids;
}

int DBDriver::getLastNodeId() const
{
	return this->getLastId("Node");
}

int DBDriver::getLastSensorId() const
{
	return this->getLastId("Sensor");
}

int DBDriver::getLastId(const std::string & tableName) const
{
	int id = 0;
	if(_ppDb)
	{
		UDEBUG("get last id from table \"%s\"", tableName.c_str());
		UTimer timer;
		timer.start();
		int rc = SQLITE_OK;
		sqlite3_stmt * ppStmt = 0;
		std::stringstream query;

		query << "SELECT max(id) "
			  << "FROM " << tableName
			  << ";";

		rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

		// Process the result if one
		rc = sqlite3_step(ppStmt);
		if(rc == SQLITE_ROW)
		{
			id = sqlite3_column_int(ppStmt, 0); // Signature Id
			rc = sqlite3_step(ppStmt);
			UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
		}
		else
		{
			ULOGGER_ERROR("No result !?! from the DB");
		}

		// Finalize (delete) the statement
		rc = sqlite3_finalize(ppStmt);
		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

		UDEBUG("Time=%fs", timer.ticks());
	}
	return id;
}

// default onlyWithActions = false
std::set<int> DBDriver::getNeighborIds(int nodeId, bool onlyWithActions) const
{
	UScopeMutex scopeMutex(_dbMutex);
	std::set<int> neighbors;
	if(_ppDb)
	{
		UTimer timer;
		timer.start();
		int rc = SQLITE_OK;
		sqlite3_stmt * ppStmt = 0;
		std::stringstream query;

		query << "SELECT DISTINCT to_id FROM Link "
			  << "WHERE from_id = " << nodeId
			  << " AND type = 0";

		if(onlyWithActions)
		{
			query << " AND actuator_id > 0";
		}

		rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

		// Process the result if one
		rc = sqlite3_step(ppStmt);
		while(rc == SQLITE_ROW)
		{
			neighbors.insert(sqlite3_column_int(ppStmt, 0)); // nid
			rc = sqlite3_step(ppStmt);
		}
		UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

		// Finalize (delete) the statement
		rc = sqlite3_finalize(ppStmt);
		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

		if(neighbors.size() == 0)
		{
			UERROR("No neighbors loaded from node %d", nodeId);
		}
	}
	return neighbors;
}

int DBDriver::getWeight(int nodeId) const
{
	UScopeMutex scopeMutex(_dbMutex);
	int weight = 0;
	if(_ppDb)
	{
		UTimer timer;
		timer.start();
		int rc = SQLITE_OK;
		sqlite3_stmt * ppStmt = 0;
		std::stringstream query;

		query << "SELECT weight FROM node WHERE id =  "
			  << nodeId
			  << ";";

		rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

		// Process the result if one
		rc = sqlite3_step(ppStmt);
		if(rc == SQLITE_ROW)
		{
			weight= sqlite3_column_int(ppStmt, 0); // weight
			rc = sqlite3_step(ppStmt);
		}
		UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

		// Finalize (delete) the statement
		rc = sqlite3_finalize(ppStmt);
		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
	}
	return weight;
}

void DBDriver::getLoopClosureIds(int nodeId, std::set<int> & loopIds, std::set<int> & childIds) const
{
	UScopeMutex scopeMutex(_dbMutex);
	loopIds.clear();
	childIds.clear();
	if(_ppDb)
	{
		UTimer timer;
		timer.start();
		int rc = SQLITE_OK;
		sqlite3_stmt * ppStmt = 0;
		std::stringstream query;

		query << "SELECT to_id, type FROM Link WHERE from_id =  "
			  << nodeId
			  << " AND type > 0"
			  << ";";

		rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

		int toId = 0;
		int type;
		// Process the result if one
		rc = sqlite3_step(ppStmt);
		while(rc == SQLITE_ROW)
		{
			int index = 0;
			toId = sqlite3_column_int(ppStmt, index++);
			type = sqlite3_column_int(ppStmt, index++);

			if(nodeId == toId)
			{
				UERROR("Loop links cannot be auto-reference links (node=%d)", toId);
			}
			else if(type == 1)
			{
				UDEBUG("Load link from %d to %d, type=%d", nodeId, toId, 1);
				//loop id
				loopIds.insert(toId);
			}
			else if(type == 2)
			{
				UDEBUG("Load link from %d to %d, type=%d", nodeId, toId, 2);
				//loop id
				childIds.insert(toId);
			}
			rc = sqlite3_step(ppStmt);
		}
		UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

		// Finalize (delete) the statement
		rc = sqlite3_finalize(ppStmt);
		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

		UDEBUG("time =%fs", timer.getElapsedTime());
	}
}

Node * DBDriver::loadNode(int nodeId) const
{
	UScopeMutex scopeMutex(_dbMutex);
	ULOGGER_DEBUG("node");
	Node * node = 0;
	if(_ppDb)
	{
		int weight = 0;
		UTimer timer;
		timer.start();
		int rc = SQLITE_OK;
		sqlite3_stmt * ppStmt = 0;
		std::stringstream query;

		query << "SELECT weight "
			  << "FROM Node "
			  << "WHERE id=" << nodeId << ";";

		rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());


		// Process the result if one
		rc = sqlite3_step(ppStmt);
		if(rc == SQLITE_ROW)
		{
			int index = 0;
			weight = sqlite3_column_int(ppStmt, index++); // weight
			rc = sqlite3_step(ppStmt);
			UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
		}
		else
		{
			UFATAL("No result !?! from the DB, node=%d",nodeId);
		}


		// Finalize (delete) the statement
		rc = sqlite3_finalize(ppStmt);
		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

		UDEBUG("Time=%fs", timer.ticks());

		//get loop / child links
		std::set<int> loopIds;
		std::set<int> childIds;
		this->getLoopClosureIds(nodeId, loopIds, childIds);

		node = new Node(nodeId);

		node->setWeight(weight);
		node->setLoopClosureIds(loopIds);
		node->setChildLoopClosureIds(childIds);
		node->addNeighbors(this->getNeighbors(nodeId));
		node->setSensors(this->getSensors(node->id()));
		node->setSaved(true);
		node->setModified(false);
	}
	return node;
}

std::vector<int> DBDriver::getSensors(int nodeId) const
{
	UScopeMutex scopeMutex(_dbMutex);
	std::vector<int> sensors;
	ULOGGER_DEBUG("SensorimotorNode");
	if(_ppDb && nodeId)
	{
		UTimer timer;
		timer.start();
		int rc = SQLITE_OK;
		sqlite3_stmt * ppStmt = 0;
		std::stringstream query;

		// Get the map from node
		query << "SELECT sensor_id "
			  << "FROM Node_To_Sensor "
			  << "WHERE node_id=" << nodeId
			  << " ORDER BY num";

		rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

		int id;

		// Process the result if one
		rc = sqlite3_step(ppStmt);
		while(rc == SQLITE_ROW)
		{
			int index = 0;
			id = sqlite3_column_int(ppStmt, index++);
			sensors.push_back(id);
			rc = sqlite3_step(ppStmt);
		}

		UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

		// Finalize (delete) the statement
		rc = sqlite3_finalize(ppStmt);
		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

		ULOGGER_DEBUG("Time=%fs", timer.ticks());
	}
	return sensors;
}

std::map<int, Sensor*> DBDriver::loadLastSensors() const
{
	UScopeMutex scopeMutex(_dbMutex);
	std::map<int, Sensor*> sensors;
	ULOGGER_DEBUG("sensors");
	if(_ppDb)
	{
		UTimer timer;
		timer.start();
		int rc = SQLITE_OK;
		sqlite3_stmt * ppStmt = 0;
		std::stringstream query;
		std::vector<float> descriptor;

		// Get the map from node
		query << "SELECT id, type, data "
			  << "FROM Sensor "
			  << "WHERE time_enter >= (SELECT MAX(time_enter) FROM Statistics) "
			  << "ORDER BY id;";

		rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

		const void * data = 0;
		int dataSize = 0;
		int type;
		int id;

		// Process the result if one
		rc = sqlite3_step(ppStmt);
		while(rc == SQLITE_ROW)
		{
			int index = 0;
			id = sqlite3_column_int(ppStmt, index++);
			type = sqlite3_column_int(ppStmt, index++);
			data = sqlite3_column_blob(ppStmt, index);
			dataSize = sqlite3_column_bytes(ppStmt, index++);
			//Create the sensor
			if(dataSize>4 && data)
			{
				descriptor = std::vector<float>(dataSize/sizeof(float));
				memcpy(&(descriptor[0]), data, dataSize);
			}
			else
			{
				descriptor = std::vector<float>();
			}

			Sensor * s = new Sensor(id, descriptor, (Sensor::Type)type);
			s->setSaved(true);
			sensors.insert(sensors.end(), std::pair<int, Sensor*>(id, s));

			rc = sqlite3_step(ppStmt);
		}

		UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

		// Finalize (delete) the statement
		rc = sqlite3_finalize(ppStmt);
		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

		ULOGGER_DEBUG("Time=%fs", timer.ticks());
	}
	return sensors;

}

std::list<Node *> DBDriver::loadNodes(const std::list<int> & ids) const
{
	UScopeMutex scopeMutex(_dbMutex);
	std::list<Node *> nodes;
	ULOGGER_DEBUG("count=%d", ids.size());
	if(_ppDb && ids.size())
	{
		UTimer timer;
		timer.start();
		int rc = SQLITE_OK;
		sqlite3_stmt * ppStmt = 0;
		std::stringstream query;
		unsigned int loaded = 0;

		// Prepare the query...
		query << "SELECT weight, sensor_id "
			  << "FROM Node "
			  << "INNER JOIN Node_To_Sensor "
			  << "ON Node.id = node_id "
			  << "WHERE Node.id=? "
			  << "ORDER BY num";
		query << ";";

		rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

		for(std::list<int>::const_iterator iter=ids.begin(); iter!=ids.end(); ++iter)
		{
			ULOGGER_DEBUG("Loading %d...", *iter);
			// bind id
			rc = sqlite3_bind_int(ppStmt, 1, *iter);
			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

			int weight = 0;
			int sensorId;
			std::vector<int> sensors;

			// Process the result if one
			rc = sqlite3_step(ppStmt);
			while(rc == SQLITE_ROW)
			{
				int index = 0;
				weight = sqlite3_column_int(ppStmt, index++);
				sensorId = sqlite3_column_int(ppStmt, index++);
				sensors.push_back(sensorId);

				rc = sqlite3_step(ppStmt);
			}

			if(*iter)
			{
				ULOGGER_DEBUG("Creating %d", *iter);
				Node * node = new Node(*iter, sensors);
				if(node)
				{
					node->setWeight(weight);
					node->setSaved(true);
					nodes.push_back(node);
				}
				else
				{
					UFATAL("Cannot allocate memory !!!");
				}
				++loaded;
			}

			UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

			//reset
			rc = sqlite3_reset(ppStmt);
			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
		}

		// Finalize (delete) the statement
		rc = sqlite3_finalize(ppStmt);
		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

		UINFO("Time=%fs", timer.ticks());

		this->loadLinks(nodes);
		for(std::list<Node*>::iterator iter = nodes.begin(); iter!=nodes.end(); ++iter)
		{
			(*iter)->setModified(false);
		}
		UINFO("Time load neighbors=%fs", timer.ticks());

		if(ids.size() != loaded)
		{
			UERROR("Some nodes not found in database");
		}
	}
	return nodes;
}


std::list<Node *> DBDriver::loadLastNodes() const
{
	UScopeMutex scopeMutex(_dbMutex);
	std::list<Node *> nodes;
	ULOGGER_DEBUG("");
	if(_ppDb)
	{
		UTimer timer;
		timer.start();
		int rc = SQLITE_OK;
		sqlite3_stmt * ppStmt = 0;
		std::stringstream query;
		std::list<int> ids;

		// Get the map from node and visual words
		query << "SELECT n.id "
				 "FROM Node AS n "
				 "WHERE n.time_enter >= (SELECT MAX(time_enter) FROM Statistics) "
				 "ORDER BY n.id;"
				 /*"AND s.parentId IS NULL;"*/;

		rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

		// Process the result if one
		rc = sqlite3_step(ppStmt);
		while(rc == SQLITE_ROW)
		{
			ids.push_back(sqlite3_column_int(ppStmt, 0)); 	// node id
			rc = sqlite3_step(ppStmt); // next result...
		}

		UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

		// Finalize (delete) the statement
		rc = sqlite3_finalize(ppStmt);
		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

		ULOGGER_DEBUG("Loading %d nodes...", ids.size());
		Node * node = 0;
		int count = 0;
		for(std::list<int>::iterator i=ids.begin(); i!=ids.end(); ++i)
		{
			node = this->loadNode(*i);
			if(node)
			{
				++count;
				nodes.push_back(node);
			}
		}

		ULOGGER_DEBUG("loaded=%d, Time=%fs", count, timer.ticks());
	}
	return nodes;
}

std::map<int, Sensor *> DBDriver::loadSensors(const std::set<int> & sensorIds, bool withRawData) const
{
	UScopeMutex scopeMutex(_dbMutex);
	std::map<int, Sensor *> sensors;
	ULOGGER_DEBUG("size=%d", sensorIds.size());
	if(_ppDb && sensorIds.size())
	{
		UTimer timer;
		timer.start();
		int rc = SQLITE_OK;
		sqlite3_stmt * ppStmt = 0;
		std::stringstream query;
		std::set<int> loaded;

		if(!withRawData)
		{
			// Load without raw data
			query << "SELECT type, data "
					 "FROM Sensor "
					 "WHERE id = ?;";
		}
		else
		{
			query << "SELECT type, data, raw_width, raw_height, raw_data_type, raw_compressed, raw_data "
				  << "FROM Sensor "
				  << "WHERE id = ?;";
		}

		rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

		for(std::set<int>::const_iterator iter=sensorIds.begin(); iter!=sensorIds.end(); ++iter)
		{
			// bind id
			rc = sqlite3_bind_int(ppStmt, 1, *iter);
			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

			// Process the result if one
			rc = sqlite3_step(ppStmt);
			if(rc == SQLITE_ROW)
			{
				int index=0;
				int type = sqlite3_column_int(ppStmt, index++);
				const void * data = sqlite3_column_blob(ppStmt, index);
				int dataSize = sqlite3_column_bytes(ppStmt, index++);

				//Create the descriptor
				std::vector<float> descriptor;
				if(dataSize>4 && data)
				{
					descriptor = std::vector<float>(dataSize/sizeof(float));
					memcpy(&(descriptor[0]), data, dataSize);
				}
				else
				{
					descriptor = std::vector<float>();
				}

				cv::Mat rawData;
				if(withRawData)
				{
					// raw data
					int width = sqlite3_column_int(ppStmt, index++);
					int height = sqlite3_column_int(ppStmt, index++);
					int dataType = sqlite3_column_int(ppStmt, index++);
					int compressed = sqlite3_column_int(ppStmt, index++);

					data = sqlite3_column_blob(ppStmt, index);
					dataSize = sqlite3_column_bytes(ppStmt, index++);

					//Create the image
					if(dataSize>4 && data)
					{
						if(compressed)
						{
							std::vector<unsigned char> compressed(dataSize);
							memcpy(compressed.data(), data, dataSize);
		#if CV_MAJOR_VERSION >=2 and CV_MINOR_VERSION >=4
							rawData = cv::imdecode(compressed, cv::IMREAD_UNCHANGED);
		#else
							rawData = cv::imdecode(compressed, -1);
		#endif
							if(dataType != rawData.type() || width != rawData.cols || height != rawData.rows)
							{
								UFATAL("dataType != dataMat.type() || width != dataMat.cols || height != dataMat.rows");
							}
						}
						else
						{
							rawData = cv::Mat(height, width, dataType);
							memcpy(rawData.data, data, dataSize);
						}
					}
				}

				Sensor * s = new Sensor(*iter, descriptor, (Sensor::Type)type, rawData);
				s->setSaved(true);
				sensors.insert(sensors.end(), std::pair<int, Sensor*>(*iter, s));
				loaded.insert(loaded.end(), *iter);

				rc = sqlite3_step(ppStmt);
			}

			UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

			rc = sqlite3_reset(ppStmt);
			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
		}

		// Finalize (delete) the statement
		rc = sqlite3_finalize(ppStmt);
		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

		ULOGGER_DEBUG("Time=%fs", timer.ticks());

		if(sensorIds.size() != loaded.size())
		{
			for(std::set<int>::const_iterator iter = sensorIds.begin(); iter!=sensorIds.end(); ++iter)
			{
				if(loaded.find(*iter) == loaded.end())
				{
					UDEBUG("Not found sensor %d", *iter);
				}
			}
			UERROR("Query (%d) doesn't match loaded sensors (%d)", sensorIds.size(), loaded.size());
		}
	}
	return sensors;
}

NeighborsMultiMap DBDriver::getNeighbors(int nodeId) const
{
	UScopeMutex scopeMutex(_dbMutex);
	NeighborsMultiMap neighbors;
	if(_ppDb)
	{
		UTimer timer;
		timer.start();
		int rc = SQLITE_OK;
		sqlite3_stmt * ppStmt = 0;
		std::stringstream query;

		query << "SELECT to_id, actuator_id, num, sensor_history, sensor_history_step, Actuator.type, width, height, data_type, data FROM Link "
		      << "LEFT JOIN Actuator "
		      << "ON Actuator.id = actuator_id "
		      << "WHERE from_id = " << nodeId
			  << " AND Link.type = 0"
			  << " ORDER BY to_id, actuator_id, num";

		rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

		int toId = -1;
		int actuatorId = -1;
		int actuatorNum = -1;
		int actuatorType;
		int actuatorWidth;
		int actuatorHeight;
		int actuatorDataType;
		const void * data = 0;
		int dataSize = 0;
		std::list<Actuator> actuators;
		std::vector<int> sensorHistoryMerged;
		int sensorHistoryStep = 0;;

		// Process the result if one
		rc = sqlite3_step(ppStmt);
		while(rc == SQLITE_ROW)
		{
			int index = 0;

			toId = sqlite3_column_int(ppStmt, index++);
			actuatorId = sqlite3_column_int(ppStmt, index++);
			actuatorNum = sqlite3_column_int(ppStmt, index++);

			// sensor_history
			dataSize = sqlite3_column_bytes(ppStmt, index);
			data = sqlite3_column_blob(ppStmt, index++);
			if(dataSize>4)
			{
				sensorHistoryMerged = std::vector<int>(dataSize/sizeof(int));
				memcpy(sensorHistoryMerged.data(), data, dataSize);
			}
			sensorHistoryStep = sqlite3_column_int(ppStmt, index++);

			if(actuatorId)
			{
				actuatorType = sqlite3_column_int(ppStmt, index++);
				actuatorWidth = sqlite3_column_int(ppStmt, index++);
				actuatorHeight = sqlite3_column_int(ppStmt, index++);
				actuatorDataType = sqlite3_column_int(ppStmt, index++);

				data = sqlite3_column_blob(ppStmt, index);
				dataSize = sqlite3_column_bytes(ppStmt, index++);

				//Create the actuator
				if(dataSize>4 && data)
				{
					cv::Mat dataMat(actuatorHeight, actuatorWidth, actuatorDataType);
					memcpy(dataMat.data, data, dataSize);
					actuators.push_back(Actuator(dataMat, (Actuator::Type)actuatorType, actuatorNum));
				}
			}

			std::list<std::vector<int> > sensorHistory;
			if(sensorHistoryStep>0 && sensorHistoryMerged.size())
			{
				for(unsigned int i=0; i<sensorHistoryMerged.size(); i+=sensorHistoryStep)
				{
					sensorHistory.push_back(std::vector<int>(sensorHistoryStep));
					memcpy(sensorHistory.back().data(), sensorHistoryMerged.data()+i, sensorHistoryStep * sizeof(int));
				}
			}
			neighbors.insert(neighbors.end(), std::pair<int, NeighborLink>(toId, NeighborLink(toId, sensorHistory, actuators, actuatorId)));
			actuators.clear();
			sensorHistoryMerged.clear();

			rc = sqlite3_step(ppStmt);
		}

		UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

		// Finalize (delete) the statement
		rc = sqlite3_finalize(ppStmt);
		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

		if(neighbors.size() == 0)
		{
			UERROR("No neighbors loaded from node %d", nodeId);
		}
	}
	return neighbors;
}

void DBDriver::loadLinks(std::list<Node *> & nodes) const
{
	if(_ppDb)
	{
		UTimer timer;
		timer.start();
		int rc = SQLITE_OK;
		sqlite3_stmt * ppStmt = 0;
		std::stringstream query;
		int totalLinksLoaded = 0;

		query << "SELECT to_id, Link.type, actuator_id, num, sensor_history, sensor_history_step, Actuator.type, width, height, data_type, data FROM Link "
			  << "LEFT JOIN Actuator "
			  << "ON Actuator.id = actuator_id "
			  << "WHERE from_id = ? "
			  << "ORDER BY to_id, actuator_id, num;";

		rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

		for(std::list<Node*>::iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
		{
			// bind id
			rc = sqlite3_bind_int(ppStmt, 1, (*iter)->id());
			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

			int toId = -1;
			int linkType = -1;
			int actuatorId = -1;
			int actuatorNum = -1;
			int actuatorType;
			int actuatorWidth;
			int actuatorHeight;
			int actuatorDataType;
			const void * data = 0;
			int dataSize = 0;
			std::list<Actuator> actuators;
			std::vector<int> sensorHistoryMerged;
			int sensorHistoryStep=-1;
			NeighborsMultiMap neighbors;
			std::set<int> loopIds;
			std::set<int> childIds;

			// Process the result if one
			rc = sqlite3_step(ppStmt);
			while(rc == SQLITE_ROW)
			{
				int index = 0;

				toId = sqlite3_column_int(ppStmt, index++);
				linkType = sqlite3_column_int(ppStmt, index++);
				actuatorId = sqlite3_column_int(ppStmt, index++);
				actuatorNum = sqlite3_column_int(ppStmt, index++);

				// baseIds
				dataSize = sqlite3_column_bytes(ppStmt, index);
				data = sqlite3_column_blob(ppStmt, index++);
				if(dataSize>4)
				{
					sensorHistoryMerged = std::vector<int>(dataSize/sizeof(int));
					memcpy(sensorHistoryMerged.data(), data, dataSize);
				}
				sensorHistoryStep = sqlite3_column_int(ppStmt, index++);

				if(actuatorId)
				{
					actuatorType = sqlite3_column_int(ppStmt, index++);
					actuatorWidth = sqlite3_column_int(ppStmt, index++);
					actuatorHeight = sqlite3_column_int(ppStmt, index++);
					actuatorDataType = sqlite3_column_int(ppStmt, index++);

					data = sqlite3_column_blob(ppStmt, index);
					dataSize = sqlite3_column_bytes(ppStmt, index++);

					//Create the actuator
					if(dataSize>4 && data)
					{
						cv::Mat dataMat(actuatorHeight, actuatorWidth, actuatorDataType);
						memcpy(dataMat.data, data, dataSize);
						actuators.push_back(Actuator(dataMat, (Actuator::Type)actuatorType, actuatorNum));
					}
				}

				std::list<std::vector<int> > sensorHistory;
				if(sensorHistoryStep>0 && sensorHistoryMerged.size())
				{
					for(unsigned int i=0; i<sensorHistoryMerged.size(); i+=sensorHistoryStep)
					{
						sensorHistory.push_back(std::vector<int>(sensorHistoryStep));
						memcpy(sensorHistory.back().data(), sensorHistoryMerged.data()+i, sensorHistoryStep * sizeof(int));
					}
				}

				if(linkType == 1)
				{
					UDEBUG("Load link from %d to %d, type=%d", (*iter)->id(), toId, 1);
					loopIds.insert(toId);
				}
				else if(linkType == 2)
				{
					UDEBUG("Load link from %d to %d, type=%d", (*iter)->id(), toId, 2);
					childIds.insert(toId);
				}
				else if(linkType == 0)
				{
					UDEBUG("Load link from %d to %d, type=%d", (*iter)->id(), toId, 0);
					neighbors.insert(neighbors.end(), std::pair<int, NeighborLink>(toId, NeighborLink(toId, sensorHistory, actuators, actuatorId)));
					if(actuatorId)
					{
						if(actuators.size() == 0 || sensorHistory.size() == 0)
						{
							UERROR("Node %d, actuatorId=%d, actuators size=%d, sensoryHistory size=%d", (*iter)->id(), actuatorId, (int)actuators.size(), (int)sensorHistory.size());
						}
					}
				}
				actuators.clear();
				sensorHistoryMerged.clear();

				++totalLinksLoaded;
				rc = sqlite3_step(ppStmt);
			}

			UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

			// add links
			(*iter)->addNeighbors(neighbors);
			(*iter)->setLoopClosureIds(loopIds);
			(*iter)->setChildLoopClosureIds(childIds);

			//reset
			rc = sqlite3_reset(ppStmt);
			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
			UINFO("time=%fs, node=%d, neighbors.size=%d, loopIds=%d, childIds=%d", (*iter)->id(), timer.ticks(), neighbors.size(), loopIds.size(), childIds.size());
		}

		// Finalize (delete) the statement
		rc = sqlite3_finalize(ppStmt);
		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
	}
}

NeighborLink * DBDriver::loadLink(int actuatorId) const
{
	NeighborLink * link = 0;
	if(_ppDb)
	{
		UTimer timer;
		timer.start();
		int rc = SQLITE_OK;
		sqlite3_stmt * ppStmt = 0;
		std::stringstream query;

		query << "SELECT to_id, sensor_history, sensor_history_step "
			  << "FROM Link "
			  << "WHERE actuator_id = " << actuatorId;

		rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

		// Process the result if one
		rc = sqlite3_step(ppStmt);
		if(rc == SQLITE_ROW)
		{
			int index = 0;

			int toId = sqlite3_column_int(ppStmt, index++);

			// history
			std::vector<int> sensorHistoryMerged;
			int dataSize = sqlite3_column_bytes(ppStmt, index);
			const void * data = sqlite3_column_blob(ppStmt, index++);
			if(dataSize>4)
			{
				sensorHistoryMerged = std::vector<int>(dataSize/sizeof(int));
				memcpy(sensorHistoryMerged.data(), data, dataSize);
			}
			int sensorHistoryStep = sqlite3_column_int(ppStmt, index++);

			std::list<std::vector<int> > sensorHistory;
			if(sensorHistoryStep>0 && sensorHistoryMerged.size())
			{
				for(unsigned int i=0; i<sensorHistoryMerged.size(); i+=sensorHistoryStep)
				{
					sensorHistory.push_back(std::vector<int>(sensorHistoryStep));
					memcpy(sensorHistory.back().data(), sensorHistoryMerged.data()+i, sensorHistoryStep * sizeof(int));
				}
			}

			link = new NeighborLink(toId, sensorHistory, std::list<Actuator>(), actuatorId);

			rc = sqlite3_step(ppStmt);
		}

		UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

		// Finalize (delete) the statement
		rc = sqlite3_finalize(ppStmt);
		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
	}
	return link;
}


void DBDriver::update(const std::list<Node *> & nodes) const
{
	if(_ppDb && nodes.size())
	{
		UTimer timer;
		timer.start();
		int rc = SQLITE_OK;
		sqlite3_stmt * ppStmt = 0;
		Node * node = 0;

		std::string query = "UPDATE Node SET weight=?, time_enter = DATETIME('NOW') WHERE id=?;";
		rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

		for(std::list<Node *>::const_iterator i=nodes.begin(); i!=nodes.end(); ++i)
		{
			node = *i;
			int index = 1;
			if(node)
			{
				rc = sqlite3_bind_int(ppStmt, index++, node->getWeight());
				UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

				rc = sqlite3_bind_int(ppStmt, index++, node->id());
				UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

				//step
				rc=sqlite3_step(ppStmt);
				UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

				rc = sqlite3_reset(ppStmt);
				UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
			}
		}
		// Finalize (delete) the statement
		rc = sqlite3_finalize(ppStmt);
		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

		ULOGGER_DEBUG("Update Node table, Time=%fs", timer.ticks());

		// Update links part1
		query = "DELETE FROM Link WHERE from_id=?;";
		rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

		for(std::list<Node *>::const_iterator j=nodes.begin(); j!=nodes.end(); ++j)
		{
			if((*j)->isNeighborsModified())
			{
				rc = sqlite3_bind_int(ppStmt, 1, (*j)->id());
				UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

				rc=sqlite3_step(ppStmt);
				UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

				rc = sqlite3_reset(ppStmt);
				UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
			}
		}
		// Finalize (delete) the statement
		rc = sqlite3_finalize(ppStmt);
		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

		// Update links part2
		query = queryStepLink();
		rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

		for(std::list<Node *>::const_iterator j=nodes.begin(); j!=nodes.end(); ++j)
		{
			if((*j)->isNeighborsModified())
			{
				// Save neighbor links
				const NeighborsMultiMap & neighbors = (*j)->getNeighbors();
				for(NeighborsMultiMap::const_iterator i=neighbors.begin(); i!=neighbors.end(); ++i)
				{
					stepLink(ppStmt, (*j)->id(), i->first, 0, i->second.actuatorId(), i->second.sensorHistoryIds());
				}
				// save loop closure links
				const std::set<int> & loopIds = (*j)->getLoopClosureIds();
				for(std::set<int>::const_iterator i=loopIds.begin(); i!=loopIds.end(); ++i)
				{
					stepLink(ppStmt, (*j)->id(), *i, 1, 0, std::list<std::vector<int> >());
				}
				const std::set<int> & childIds = (*j)->getChildLoopClosureIds();
				for(std::set<int>::const_iterator i=childIds.begin(); i!=childIds.end(); ++i)
				{
					stepLink(ppStmt, (*j)->id(), *i, 2, 0, std::list<std::vector<int> >());
				}
			}
		}
		// Finalize (delete) the statement
		rc = sqlite3_finalize(ppStmt);
		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
		UDEBUG("Update Neighbors Time=%fs", timer.ticks());

		if(nodes.size())
		{
			// Update sensors references
			query = queryStepSensorsChanged();
			rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

			for(std::list<Node *>::const_iterator j=nodes.begin(); j!=nodes.end(); ++j)
			{
				if((*j)->getSensorsChanged().size())
				{
					const std::map<int, int> & sensorsChanged = (*j)->getSensorsChanged();
					for(std::map<int, int>::const_iterator iter=sensorsChanged.begin(); iter!=sensorsChanged.end(); ++iter)
					{
						stepSensorsChanged(ppStmt, (*j)->id(), iter->first, iter->second);
					}
				}
			}
			// Finalize (delete) the statement
			rc = sqlite3_finalize(ppStmt);
			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
		}
		ULOGGER_DEBUG("node update=%fs", timer.ticks());
	}
}
void DBDriver::update(const std::list<Sensor *> & sensors) const
{
	if(_ppDb && sensors.size())
	{
		UTimer timer;
		timer.start();
		int rc = SQLITE_OK;
		sqlite3_stmt * ppStmt = 0;
		Sensor * s = 0;

		std::string query = "UPDATE Sensor SET time_enter = DATETIME('NOW') WHERE id=?;";
		rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

		for(std::list<Sensor *>::const_iterator i=sensors.begin(); i!=sensors.end(); ++i)
		{
			s = *i;
			int index = 1;
			if(s)
			{
				rc = sqlite3_bind_int(ppStmt, index++, s->id());
				UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

				//step
				rc=sqlite3_step(ppStmt);
				UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

				rc = sqlite3_reset(ppStmt);
				UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
			}
		}
		// Finalize (delete) the statement
		rc = sqlite3_finalize(ppStmt);
		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
	}
}

void DBDriver::save(const std::list<Node *> & nodes) const
{
	UDEBUG("");
	if(_ppDb && nodes.size())
	{
		UTimer timer;
		timer.start();
		int rc = SQLITE_OK;
		sqlite3_stmt * ppStmt = 0;

		// Signature table
		std::string query = queryStepNode();
		rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

		for(std::list<Node *>::const_iterator i=nodes.begin(); i!=nodes.end(); ++i)
		{
			stepNode(ppStmt, *i);
		}
		// Finalize (delete) the statement
		rc = sqlite3_finalize(ppStmt);
		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

		UDEBUG("Time=%fs", timer.ticks());

		// Create new entries in table Link
		query = queryStepLink();
		rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

		for(std::list<Node *>::const_iterator jter=nodes.begin(); jter!=nodes.end(); ++jter)
		{
			// Save neighbor links
			const NeighborsMultiMap & neighbors = (*jter)->getNeighbors();
			for(NeighborsMultiMap::const_iterator i=neighbors.begin(); i!=neighbors.end(); ++i)
			{
				stepLink(ppStmt, (*jter)->id(), i->first, 0, i->second.actuatorId(), i->second.sensorHistoryIds());
			}
			// save loop closure links
			const std::set<int> & loopIds = (*jter)->getLoopClosureIds();
			for(std::set<int>::const_iterator i=loopIds.begin(); i!=loopIds.end(); ++i)
			{
				stepLink(ppStmt, (*jter)->id(), *i, 1, 0, std::list<std::vector<int> >());
			}
			const std::set<int> & childIds = (*jter)->getChildLoopClosureIds();
			for(std::set<int>::const_iterator i=childIds.begin(); i!=childIds.end(); ++i)
			{
				stepLink(ppStmt, (*jter)->id(), *i, 2, 0, std::list<std::vector<int> >());
			}
		}
		// Finalize (delete) the statement
		rc = sqlite3_finalize(ppStmt);
		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

		UDEBUG("Time=%fs", timer.ticks());

		// Create new entries in table Actuator
		query = queryStepActuator();
		rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

		for(std::list<Node *>::const_iterator jter=nodes.begin(); jter!=nodes.end(); ++jter)
		{
			// Save actuators
			const NeighborsMultiMap & neighbors = (*jter)->getNeighbors();
			for(NeighborsMultiMap::const_iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter)
			{
				if(iter->second.actuatorId())
				{
					const std::list<Actuator> & actuators = iter->second.actuators();
					int k=0;
					for(std::list<Actuator>::const_iterator kter=actuators.begin(); kter!=actuators.end(); ++kter)
					{
						stepActuator(ppStmt, iter->second.actuatorId(), k++, *kter);
					}
				}
			}
		}
		// Finalize (delete) the statement
		rc = sqlite3_finalize(ppStmt);
		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

		UDEBUG("Time=%fs", timer.ticks());


		// Create new entries in table Node_To_Sensor
		query = queryStepNodeToSensor();
		rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

		for(std::list<Node *>::const_iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
		{
			const std::vector<int> & sensor = (*iter)->getSensors();
			for(unsigned int i=0; i<sensor.size(); ++i)
			{
				stepNodeToSensor(ppStmt, (*iter)->id(), sensor[i], i);
			}
		}
		// Finalize (delete) the statement
		rc = sqlite3_finalize(ppStmt);
		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

		UDEBUG("Time=%fs", timer.ticks());
	}
}

void DBDriver::save(const std::list<Sensor *> & sensors) const
{
	UDEBUG("sensors size=%d", sensors.size());
	if(_ppDb)
	{
		UTimer timer;
		timer.start();
		int rc = SQLITE_OK;
		sqlite3_stmt * ppStmt = 0;
		std::string query;

		// Create new entries in table Sensor
		if(sensors.size()>0)
		{
			query = queryStepSensor();
			rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

			for(std::list<Sensor*>::const_iterator i=sensors.begin(); i!=sensors.end(); ++i)
			{
				stepSensor(ppStmt, *(*i));
			}
			// Finalize (delete) the statement
			rc = sqlite3_finalize(ppStmt);
			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
		}

		UDEBUG("Time=%fs", timer.ticks());
	}
}

std::string DBDriver::queryStepNode() const
{
	return "INSERT INTO Node(id, weight) VALUES(?,?);";
}
void DBDriver::stepNode(sqlite3_stmt * ppStmt, const Node * s) const
{
	UDEBUG("save node %d", s->id());
	UASSERT(ppStmt != 0 && s != 0);

	int rc = SQLITE_OK;

	int index = 1;
	rc = sqlite3_bind_int(ppStmt, index++, s->id());
	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

	rc = sqlite3_bind_int(ppStmt, index++, s->getWeight());
	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

	//step
	rc=sqlite3_step(ppStmt);
	UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

	rc = sqlite3_reset(ppStmt);
	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
}

std::string DBDriver::queryStepSensor() const
{
	return "INSERT INTO Sensor(id, type, data, raw_width, raw_height, raw_data_type, raw_compressed, raw_data) VALUES(?,?,?,?,?,?,?,?);";
}
void DBDriver::stepSensor(sqlite3_stmt * ppStmt, const Sensor & sensor) const
{
	UDEBUG("saving sensor %d, type=%d", sensor.id(), sensor.type());
	UASSERT(ppStmt != 0);

	int rc = SQLITE_OK;
	std::vector<unsigned char> compressed;
	int index = 1;

	rc = sqlite3_bind_int(ppStmt, index++, sensor.id());
	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

	rc = sqlite3_bind_int(ppStmt, index++, sensor.type());
	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

	if(sensor.descriptor().size())
	{
		rc = sqlite3_bind_blob(ppStmt, index++, sensor.descriptor().data(), sensor.descriptor().size()*sizeof(float), SQLITE_STATIC);
	}
	else
	{
		rc = sqlite3_bind_zeroblob(ppStmt, index++, 4);
	}
	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

	rc = sqlite3_bind_int(ppStmt, index++, sensor.rawData().cols);
	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

	rc = sqlite3_bind_int(ppStmt, index++, sensor.rawData().rows);
	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

	rc = sqlite3_bind_int(ppStmt, index++, sensor.rawData().type());
	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

	rc = sqlite3_bind_int(ppStmt, index++, _imagesCompressed && sensor.type()==Sensor::kTypeImage?1:0);
	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

	if(sensor.rawData().total())
	{
		if(_imagesCompressed && sensor.type()==Sensor::kTypeImage)
		{
			cv::imencode(".png", sensor.rawData(), compressed);
			if(compressed.size())
			{
				rc = sqlite3_bind_blob(ppStmt, index++, compressed.data(), compressed.size()*sizeof(uchar), SQLITE_STATIC);
			}
			else
			{
				UERROR("Compression failed!");
				rc = sqlite3_bind_zeroblob(ppStmt, index++, 4);
			}
		}
		else
		{
			if(!sensor.rawData().isContinuous())
			{
				UWARN("matrix is not continuous");
			}
			rc = sqlite3_bind_blob(ppStmt, index++, sensor.rawData().data, sensor.rawData().total()*sensor.rawData().elemSize(), SQLITE_STATIC);
		}
	}
	else
	{
		rc = sqlite3_bind_zeroblob(ppStmt, index++, 4);
	}

	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

	//step
	rc=sqlite3_step(ppStmt);
	UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

	rc = sqlite3_reset(ppStmt);
	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
}

std::string DBDriver::queryStepNodeToSensor() const
{
	return "INSERT INTO Node_To_Sensor(node_id, sensor_id, num) VALUES(?,?,?);";
}
void DBDriver::stepNodeToSensor(sqlite3_stmt * ppStmt, int nodeId, int sensorId, int num) const
{
	UASSERT(ppStmt != 0);

	int rc = SQLITE_OK;
	std::vector<unsigned char> compressed;
	int index = 1;

	rc = sqlite3_bind_int(ppStmt, index++, nodeId);
	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

	rc = sqlite3_bind_int(ppStmt, index++, sensorId);
	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

	rc = sqlite3_bind_int(ppStmt, index++, num);
	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

	//step
	rc=sqlite3_step(ppStmt);
	UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

	rc = sqlite3_reset(ppStmt);
	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
}

std::string DBDriver::queryStepLink() const
{
	return "INSERT INTO Link(from_id, to_id, type, actuator_id, sensor_history, sensor_history_step) VALUES(?,?,?,?,?,?);";
}
void DBDriver::stepLink(sqlite3_stmt * ppStmt, int fromId, int toId, int type, int actuatorId, const std::list<std::vector<int> > & sensorHistory) const
{
	UASSERT(ppStmt != 0);

	if(actuatorId)
	{
		if(sensorHistory.size() == 0)
		{
			UERROR("Saving null sensor history (actuatorId=%d)", actuatorId);
		}
	}

	UDEBUG("Save link from %d to %d, type=%d, a_id=%d, sensorHistory.size()=%d", fromId, toId, type, actuatorId, (int)sensorHistory.size());
	int rc = SQLITE_OK;
	int index = 1;

	rc = sqlite3_bind_int(ppStmt, index++, fromId);
	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

	rc = sqlite3_bind_int(ppStmt, index++, toId);
	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

	rc = sqlite3_bind_int(ppStmt, index++, type);
	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

	rc = sqlite3_bind_int(ppStmt, index++, actuatorId);
	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

	int sensoryHistoryStep = 0;
	std::vector<int> mergedHistory;
	if(sensorHistory.size())
	{
		sensoryHistoryStep = sensorHistory.begin()->size();
		if(sensoryHistoryStep>0)
		{
			mergedHistory = std::vector<int>(sensoryHistoryStep*sensorHistory.size());
			int i=0;
			for(std::list<std::vector<int> >::const_iterator iter=sensorHistory.begin(); iter!=sensorHistory.end(); ++iter)
			{
				memcpy(mergedHistory.data()+i, iter->data(), sensoryHistoryStep*sizeof(int));
				i+=sensoryHistoryStep;
			}
		}
		else
		{
			UERROR("sensorHistory.size()=%d but sensoryHistoryStep is null", (int)sensorHistory.size());
		}
	}

	if(sensoryHistoryStep)
	{
		rc = sqlite3_bind_blob(ppStmt, index++, mergedHistory.data(), mergedHistory.size()*sizeof(int), SQLITE_STATIC);
	}
	else
	{
		if(actuatorId)
		{
			UERROR("sensoryHistoryStep=0 but actuatorId=%d", actuatorId);
		}
		rc = sqlite3_bind_zeroblob(ppStmt, index++, 4);
	}
	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

	rc = sqlite3_bind_int(ppStmt, index++, sensoryHistoryStep);
	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

	rc=sqlite3_step(ppStmt);
	UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

	rc = sqlite3_reset(ppStmt);
	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
}

std::string DBDriver::queryStepActuator() const
{
	return "REPLACE INTO Actuator(id, num, type, width, height, data_type, data) VALUES(?,?,?,?,?,?,?);";
}
void DBDriver::stepActuator(sqlite3_stmt * ppStmt, int id, int num, const Actuator & actuator) const
{
	UASSERT(ppStmt != 0 && actuator.data().total() > 0);

	int rc = SQLITE_OK;
	std::vector<unsigned char> compressed;
	int index = 1;

	rc = sqlite3_bind_int(ppStmt, index++, id);
	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

	rc = sqlite3_bind_int(ppStmt, index++, num);
	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

	rc = sqlite3_bind_int(ppStmt, index++, actuator.type());
	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

	rc = sqlite3_bind_int(ppStmt, index++, actuator.data().cols);
	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

	rc = sqlite3_bind_int(ppStmt, index++, actuator.data().rows);
	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

	rc = sqlite3_bind_int(ppStmt, index++, actuator.data().type());
	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

	if(!actuator.data().isContinuous())
	{
		UWARN("matrix is not continuous");
	}
	rc = sqlite3_bind_blob(ppStmt, index++, actuator.data().data, actuator.data().total()*actuator.data().elemSize(), SQLITE_STATIC);
	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

	//step
	rc=sqlite3_step(ppStmt);
	UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

	rc = sqlite3_reset(ppStmt);
	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
}

std::string DBDriver::queryStepSensorsChanged() const
{
	return "UPDATE Node_To_Sensor SET sensor_id = ? WHERE sensor_id = ? AND node_id = ?;";
}
void DBDriver::stepSensorsChanged(sqlite3_stmt * ppStmt, int nodeId, int oldSensorId, int newSensorId) const
{
	UASSERT(ppStmt != 0);

	int rc = SQLITE_OK;
	int index = 1;

	rc = sqlite3_bind_int(ppStmt, index++, newSensorId);
	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

	rc = sqlite3_bind_int(ppStmt, index++, oldSensorId);
	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

	rc = sqlite3_bind_int(ppStmt, index++, nodeId);
	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

	rc=sqlite3_step(ppStmt);
	UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

	rc = sqlite3_reset(ppStmt);
	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
}

} // namespace rtabmap
